/*
 * @Author: junhuizhou
 * @Date: 2020-12-30 10:37:08
 * @LastEditor: junhuizhou
 * @LastEditTime: 2021-01-11 21:27:31
 * @Description: header
 * @FilePath: \fzhtq\main.c
 */

#include <stdio.h>
#include <math.h>
#include "constant.h"
#include "attprop.h"
#include "caldrive.h"
#include "ctltask.h"
#include "calmatrix.h"
#include "caltransmat.h"

int main()
{
    /*-------------------- 文件写入 --------------------*/
    FILE* pfile = NULL;
    pfile = fopen("data.csv", "w");
    if(pfile == NULL)
    {
        return -1;
    }

    /*-------------------- 初始化 --------------------*/
    /*航天器参数初始化*/
    double JM[3][3] = {{19+20*2.020,0,0}, {0,19+15*2.020,0}, {0,0,19+10*2.020}};
    double JMI[3][3] = {{0.0168,0,0}, {0,0.0203,0}, {0,0,0.0255}};
    /*姿态初始化*/
    // double initAng[3] = {2020, -2020, 2020};       // deg
    double initAng[3] = {40, 40, 40};       // deg
    double initAngRate[3] = {360, 160, -120};   // deg/s
    // double initAngRate[3] = {0, 0, 0};   // deg/s
    double att[7] = {0.0};                      // [q0,q1,q2,q3,wx,wy,wz],rad/s
    double euler[3] = {0.0, 0.0, 0.0};          // rad
    double eulerRate[3]  = {0.0, 0.0, 0.0};     // rad
    for(int i=0; i<3; i++)
    {
        euler[i] = initAng[i]*DEG2RAD;
        eulerRate[i] = initAngRate[i]*DEG2RAD;
    }
    AttInit(att, euler, eulerRate);
    /*喷气推力器初始化*/
    /**
	 * 推力器的力矩方向（正负代表方向，0.5是力臂）
	 * +X:2,5;
	 * -X:1,6;
	 * +Y:9;
	 * -Y:10;
	 * +Z:3,8;
	 * -Z:4,7;
	 */
	double thrusterAtt[3][10] =
		{
			{-0.5, 0.5, 0.0, 0.0, 0.5, -0.5, 0.0, 0.0, 0.0, 0.0},
			{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, -0.5},
			{0.0, 0.0, 0.5, -0.5, 0.0, 0.0, -0.5, 0.5, 0.0, 0.0}};
    double PS_PTime[10] = {0.0};
    double M_PS[3] = {0.0, 0.0, 0.0};
    /*动量轮初始化*/
    double WhlSpdNow[4] = {0.0};                /*当前转速*/
    double WhlSpdDsrd[4] = {0.0};               /*期望转速*/
    double WhlSpdDsrdLst[4] = {0.0};            /*上个控制周期期望转速*/
    double whlAxiAngMom[3] = {0.0, 0.0, 0.0};   /*动量轮角动量*/
    double WhlAxisTq[3] = {0.0, 0.0, 0.0};      /*动量轮控制力拒*/
    
    /*-------------------- 主循环 --------------------*/
    double ha = EnvSim_T_Time;  // s
    double simTime = 0;         // s
    int tcount = 0;
    int tcountMax = Ratio_Ctl2Sim;
    while(1)
    {
        /*数据记录*/
        fprintf(pfile, "%f,", euler[0]*RAD2DEG);
        fprintf(pfile, "%f,", euler[1]*RAD2DEG);
        fprintf(pfile, "%f,", euler[2]*RAD2DEG);
        fprintf(pfile, "%f,", eulerRate[0]*RAD2DEG);
        fprintf(pfile, "%f,", eulerRate[1]*RAD2DEG);
        fprintf(pfile, "%f,", eulerRate[2]*RAD2DEG);
        fprintf(pfile, "%f,", M_PS[0]);
        fprintf(pfile, "%f,", M_PS[1]);
        fprintf(pfile, "%f,", M_PS[2]);
        fprintf(pfile, "%f,", simTime);
        fprintf(pfile, "%f,", WhlSpdNow[0]);
        fprintf(pfile, "%f,", WhlSpdNow[1]);
        fprintf(pfile, "%f,", WhlSpdNow[2]);
        fprintf(pfile, "%f,", PS_PTime[0]);
        fprintf(pfile, "%f,", PS_PTime[5]);
        fprintf(pfile, "%f,", PS_PTime[1]);
        fprintf(pfile, "%f,", PS_PTime[4]);
        fprintf(pfile, "%f,", PS_PTime[9]);
        fprintf(pfile, "%f,", PS_PTime[8]);
        fprintf(pfile, "%f,", PS_PTime[3]);
        fprintf(pfile, "%f,", PS_PTime[6]);
        fprintf(pfile, "%f,", PS_PTime[2]);
        fprintf(pfile, "%f\n", PS_PTime[7]);
        /*时间计数*/
        tcount++;
        simTime += ha;
        /*计算动量轮参数*/
        CalWhl(WhlSpdNow, WhlSpdDsrd, WhlSpdDsrdLst, whlAxiAngMom, WhlAxisTq);
        /*计算推力器力矩*/
        double pstime[10];
        for(int i=0; i<10; i++)
        {
            pstime[i] = PS_PTime[i]/(1E+3);    /*ms转s*/
        }
        CalAttCtrlPS(M_PS, thrusterAtt, pstime);
        /*干扰力矩*/
        double Td[3] = {0.0};
        Td[0] = Td_Max*(cos(0.01*simTime) - 0.3);
        Td[1] = Td_Max*(cos(0.02*simTime)*0.3 + 0.6);
        Td[2] = Td_Max*(sin(0.02*simTime)*0.5);
        /*总力矩*/
        double Tq[3] = {0.0};
        MatAdd(Tq, WhlAxisTq, M_PS, 1, 3);
        MatAdd(Tq, Tq, Td, 1, 3);
        /*姿态动力学递推*/
        AttDynaProp(att, JM, JMI, whlAxiAngMom, Tq, ha);
        Quat2Euler(att, euler);
        eulerRate[0] = att[4];
        eulerRate[1] = att[5];
        eulerRate[2] = att[6];
        /*姿态控制*/
        if(tcount == tcountMax)
        {
            tcount = 0;
            CtlTask(euler, eulerRate, PS_PTime, WhlSpdDsrd, WhlSpdNow);
        }

        /*仿真结束条件*/
        if(simTime >= Total_Time)
        {
            break;
        }
    }
    
    /*-------------------- 关闭文件 --------------------*/
    fclose(pfile);
    pfile = NULL;
    printf("hello world. \n");
    return 0;
}